dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * shot_local_estimator.h
   3  *
   4  *  Created on: Mar 24, 2012
   5  *      Author: aitor
   6  */
   7
   8 #pragma once
   9
  10 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
  11 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
  12 #include <pcl/features/shot.h>
  13 #include <pcl/io/pcd_io.h>
  14
  15 namespace pcl
  16 {
  17   namespace rec_3d_framework
  18   {
  19     template<typename PointInT, typename FeatureT>
  20       class SHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT>
  21       {
  22
  23         using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  24         using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  25
  26         using LocalEstimator<PointInT, FeatureT>::support_radius_;
  27         using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  28         using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  29         using LocalEstimator<PointInT, FeatureT>::adaptative_MLS_;
  30
  31       public:
  32         bool
  33         estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) override
  34         {
  35
  36           if (!normal_estimator_)
  37           {
  38             PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
  39             return false;
  40           }
  41
  42           if (keypoint_extractor_.empty ())
  43           {
  44             PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
  45             return false;
  46           }
  47
  48           pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  49           pcl::MovingLeastSquares<PointInT, PointInT> mls;
  50           if(adaptative_MLS_) {
  51             typename search::KdTree<PointInT>::Ptr tree;
  52             Eigen::Vector4f centroid_cluster;
  53             pcl::compute3DCentroid (*in, centroid_cluster);
  54             float dist_to_sensor = centroid_cluster.norm();
  55             float sigma = dist_to_sensor * 0.01f;
  56             mls.setSearchMethod(tree);
  57             mls.setSearchRadius (sigma);
  58             mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE);
  59             mls.setUpsamplingRadius (0.002);
  60             mls.setUpsamplingStepSize (0.001);
  61           }
  62
  63           normals.reset (new pcl::PointCloud<pcl::Normal>);
  64           {
  65             pcl::ScopeTime t ("Compute normals");
  66             normal_estimator_->estimate (in, processed, normals);
  67           }
  68
  69           if(adaptative_MLS_) {
  70             mls.setInputCloud(processed);
  71
  72             PointInTPtr filtered(new pcl::PointCloud<PointInT>);
  73             mls.process(*filtered);
  74
  75             processed.reset(new pcl::PointCloud<PointInT>);
  76             normals.reset (new pcl::PointCloud<pcl::Normal>);
  77             {
  78               pcl::ScopeTime t ("Compute normals after MLS");
  79               filtered->is_dense = false;
  80               normal_estimator_->estimate (filtered, processed, normals);
  81             }
  82           }
  83
  84           //compute normals
  85           //pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  86           //normal_estimator_->estimate (in, processed, normals);
  87
  88           //compute keypoints
  89           this->computeKeypoints(processed, keypoints, normals);
  90           std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
  91
  92           //compute keypoints
  93           /*keypoint_extractor_->setInputCloud (processed);
  94           if(keypoint_extractor_->needNormals())
  95             keypoint_extractor_->setNormals(normals);
  96
  97           std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
  98
  99           keypoint_extractor_->setSupportRadius(support_radius_);
 100           keypoint_extractor_->compute (keypoints);*/
 101
 102           if (keypoints->points.empty ())
 103           {
 104             PCL_WARN("SHOTLocalEstimation :: No keypoints were found\n");
 105             return false;
 106           }
 107
 108           std::cout << keypoints->points.size() << " " << normals->points.size() << " " << processed->points.size() << std::endl;
 109           //compute signatures
 110           using SHOTEstimator = pcl::SHOTEstimation<PointInT, pcl::Normal, pcl::SHOT352>;
 111           typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
 112
 113           pcl::PointCloud<pcl::SHOT352>::Ptr shots (new pcl::PointCloud<pcl::SHOT352>);
 114
 115           SHOTEstimator shot_estimate;
 116           shot_estimate.setSearchMethod (tree);
 117           shot_estimate.setInputCloud (keypoints);
 118           shot_estimate.setSearchSurface(processed);
 119           shot_estimate.setInputNormals (normals);
 120           shot_estimate.setRadiusSearch (support_radius_);
 121           shot_estimate.compute (*shots);
 122           signatures->resize (shots->points.size ());
 123           signatures->width = static_cast<int> (shots->points.size ());
 124           signatures->height = 1;
 125
 126           int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
 127
 128           for (std::size_t k = 0; k < shots->points.size (); k++)
 129             for (int i = 0; i < size_feat; i++)
 130               signatures->points[k].histogram[i] = shots->points[k].descriptor[i];
 131
 132           return true;
 133
 134         }
 135
 136       private:
 137
 138
 139       };
 140   }
 141 }